using System;

public class ECEF
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }

    public ECEF(double x, double y, double z)
    {
        X = x;
        Y = y;
        Z = z;
    }

    public override string ToString()
    {
        return $"ECEF(X: {X:F3}, Y: {Y:F3}, Z: {Z:F3})";
    }
}

public class LLA
{
    public double Latitude { get; set; }  // 纬度 (度)
    public double Longitude { get; set; } // 经度 (度)
    public double Altitude { get; set; }  // 海拔高度 (米)

    public LLA(double latitude, double longitude, double altitude)
    {
        Latitude = latitude;
        Longitude = longitude;
        Altitude = altitude;
    }

    public override string ToString()
    {
        return $"LLA(Lat: {Latitude:F6}°, Lon: {Longitude:F6}°, Alt: {Altitude:F3}m)";
    }
}

public static class CoordinateConverter
{
    // WGS84椭球体参数
    private const double A = 6378137.0;           // 长半轴 (米)
    private const double B = 6356752.314245;      // 短半轴 (米)
    private const double F = 1.0 / 298.257223563; // 扁率
    private const double E2 = 1.0 - (B * B) / (A * A); // 第一偏心率的平方
    private const double E_P2 = E2 / (1.0 - E2);       // 第二偏心率的平方

    /// <summary>
    /// 将ECEF坐标转换为LLA坐标 (WGS84)
    /// </summary>
    /// <param name="ecef">ECEF坐标</param>
    /// <returns>LLA坐标</returns>
    public static LLA ECEFToLLA(ECEF ecef)
    {
        double x = ecef.X;
        double y = ecef.Y;
        double z = ecef.Z;

        double lon = Math.Atan2(y, x);

        double p = Math.Sqrt(x * x + y * y);
        double theta = Math.Atan2(z * A, p * B);

        double sinTheta = Math.Sin(theta);
        double cosTheta = Math.Cos(theta);

        double lat = Math.Atan2(z + E_P2 * B * Math.Pow(sinTheta, 3),
                               p - E2 * A * Math.Pow(cosTheta, 3));

        double sinLat = Math.Sin(lat);
        double cosLat = Math.Cos(lat);

        double N = A / Math.Sqrt(1 - E2 * sinLat * sinLat);
        double alt = p / cosLat - N;

        // 处理极点情况
        if (cosLat < 1e-10)
        {
            alt = Math.Abs(z) - B;
        }

        // 转换为度
        lat = lat * 180.0 / Math.PI;
        lon = lon * 180.0 / Math.PI;

        return new LLA(lat, lon, alt);
    }

    /// <summary>
    /// 将LLA坐标转换为ECEF坐标 (WGS84)
    /// </summary>
    /// <param name="lla">LLA坐标</param>
    /// <returns>ECEF坐标</returns>
    public static ECEF LLAToECEF(LLA lla)
    {
        double lat = lla.Latitude * Math.PI / 180.0;
        double lon = lla.Longitude * Math.PI / 180.0;
        double alt = lla.Altitude;

        double sinLat = Math.Sin(lat);
        double cosLat = Math.Cos(lat);
        double sinLon = Math.Sin(lon);
        double cosLon = Math.Cos(lon);

        double N = A / Math.Sqrt(1 - E2 * sinLat * sinLat);

        double x = (N + alt) * cosLat * cosLon;
        double y = (N + alt) * cosLat * sinLon;
        double z = (N * (1 - E2) + alt) * sinLat;

        return new ECEF(x, y, z);
    }
}

class Program
{
    static void Main()
    {
        Console.WriteLine("ECEF转LLA坐标转换测试");
        Console.WriteLine("=====================\n");

        // 测试用例1：格林尼治天文台附近
        TestConversion(
            "测试用例1: 格林尼治天文台附近",
            new ECEF(3980601.0, -0.0, 4966863.0),
            new LLA(51.4779, 0.0, 45.0)
        );

        // 测试用例2：北极点
        TestConversion(
            "测试用例2: 北极点附近",
            new ECEF(0.0, 0.0, 6356752.0),
            new LLA(90.0, 0.0, 0.0)
        );

        // 测试用例3：赤道上的点
        TestConversion(
            "测试用例3: 赤道上的点",
            new ECEF(6378137.0, 0.0, 0.0),
            new LLA(0.0, 0.0, 0.0)
        );

        // 测试用例4：北京天安门
        TestConversion(
            "测试用例4: 北京天安门",
            new ECEF(-2148344.0, 4426641.0, 4044655.0),
            new LLA(39.9087, 116.3975, 44.0)
        );
    }

    static void TestConversion(string testName, ECEF expectedECEF, LLA expectedLLA)
    {
        Console.WriteLine($"{testName}");
        Console.WriteLine($"预期LLA: {expectedLLA}");

        // ECEF转LLA
        LLA calculatedLLA = CoordinateConverter.ECEFToLLA(expectedECEF);
        Console.WriteLine($"计算LLA: {calculatedLLA}");

        // LLA转ECEF（验证）
        ECEF calculatedECEF = CoordinateConverter.LLAToECEF(expectedLLA);
        Console.WriteLine($"预期ECEF: {expectedECEF}");
        Console.WriteLine($"计算ECEF: {calculatedECEF}");

        // 计算误差
        double llaError = CalculateLLAError(expectedLLA, calculatedLLA);
        double ecefError = CalculateECEFError(expectedECEF, calculatedECEF);

        Console.WriteLine($"LLA误差: {llaError:F6} 度, {llaError * 111000:F3} 米");
        Console.WriteLine($"ECEF误差: {ecefError:F3} 米");
        Console.WriteLine();
    }

    static double CalculateLLAError(LLA expected, LLA calculated)
    {
        double latError = Math.Abs(expected.Latitude - calculated.Latitude);
        double lonError = Math.Abs(expected.Longitude - calculated.Longitude);
        double altError = Math.Abs(expected.Altitude - calculated.Altitude) / 6378137.0; // 转换为角度近似

        return Math.Sqrt(latError * latError + lonError * lonError + altError * altError);
    }

    static double CalculateECEFError(ECEF expected, ECEF calculated)
    {
        double dx = expected.X - calculated.X;
        double dy = expected.Y - calculated.Y;
        double dz = expected.Z - calculated.Z;

        return Math.Sqrt(dx * dx + dy * dy + dz * dz);
    }
}
